//
// Created by pulsarv on 21-2-4.
//
#include <opencv2/opencv.hpp>
#include <utility>
#include <rc_system/camera_calibration.h>

#define _SILENCE_EXPERIMENTAL_FILESYSTEM_DEPRECATION_WARNING

#include <experimental/filesystem>

namespace rccore {
    namespace functions {
        Calibration::Calibration(size_t aqXnum,
                                 size_t aqYnum,
                                 size_t width,
                                 size_t height,
                                 std::string image_path,
                                 const std::string &output_xml_filename) : aqXnum(aqXnum), aqYnum(aqYnum), width(width),
                                                                           height(height),
                                                                           output_xml_filename(output_xml_filename) {
            //利用你在define当中输入的角点个数和角点长宽计算出每个角点的实际坐标
            for (int i = 0; i < aqYnum; i++) {
                for (int j = 0; j < aqXnum; j++) {
                    cv::Point3f tempPoint;
                    /* 假设标定板放在世界坐标系中z=0的平面上 */
                    tempPoint.x = i * width;
                    tempPoint.y = j * height;
                    tempPoint.z = 0;
                    objp.push_back(tempPoint);
                }
            }
            cv::glob(std::move(image_path), images);
            for (auto image_path : images) {
                slog::info << image_path << slog::endl;
            }
        }

        void Calibration::next() {
            cv::Mat frame, gray;
            slog::info << current_image_index << slog::endl;
            frame = cv::imread(images[current_image_index]);
            cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);
            m_view_function(frame);
            //寻找图片中角点
            bool success = cv::findChessboardCorners(
                    gray, cv::Size(aqXnum, aqYnum), corner_pts,
                    cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE);
            if (success) {
                //定义停止迭代的精度
                cv::TermCriteria criteria(cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 0.001);

                //进一步精细化角点
                cv::cornerSubPix(gray, corner_pts, cv::Size(11, 11), cv::Size(-1, -1), criteria);

                //在图像中画出找到的角点
                cv::drawChessboardCorners(frame, cv::Size(aqXnum, aqYnum), corner_pts, success);

                //保存找的角点坐标和对应的实际坐标
                objpoints.push_back(objp);
                imgpoints.push_back(corner_pts);
            } else {
                std::cout << "fail!" << std::endl;
            }
            IMAGE_WIDTH = gray.rows;
            IMAGE_HEIGHT = gray.cols;
            current_image_index += 1;
            m_view_function(frame);
        }

        bool Calibration::is_end() {
            return ((images.size() - 1) - current_image_index) == 0;
        }

        void Calibration::caclulate() {
            cv::Mat cameraMatrix, distCoeffs, R, T;

            //利用坐标对应关系找出相机内参和外参
            cv::calibrateCamera(
                    objpoints, imgpoints, cv::Size(IMAGE_WIDTH, IMAGE_HEIGHT), cameraMatrix, distCoeffs, R, T);

            std::cout << "cameraMatrix : " << cameraMatrix << std::endl;
            std::cout << "distCoeffs : " << distCoeffs << std::endl;

            cv::FileStorage fs(output_xml_filename, cv::FileStorage::WRITE);
            fs << "cameraMatrix" << cameraMatrix;
            fs << "distCoeffs" << distCoeffs;
            fs.release();
        }

        bool Calibration::createOutputDir(const std::string &outputPath) {
            if (!std::experimental::filesystem::is_directory(outputPath) &&
                !std::experimental::filesystem::create_directories(outputPath)) {
                slog::err << "Failed to create: " << outputPath.c_str() << slog::endl;
                return false;
            }

            return true;
        }

        void Calibration::bind_show(std::function<void(cv::Mat)> view_function) {
            m_view_function = std::move(view_function);
        }
    }
}
